// #define __KERNEL__
// #define MODULE
#include <linux/module.h>
#include <linux/fs.h>
#include <linux/uaccess.h>
// #include <rtai.h>
#include <rtai_sched.h>
#include <rtai_sem.h>
#include "rtai_shmem.h"
#include "/usr/local/include/ecrt.h"

#define MAX_PDO_ENTRIES 128
#define SHM_VAR_MAX 64

// #define INHIBIT_TIME 20 // NEW: 与 RTAI 示例一致
// static cycles_t t_last_cycle = 0, t_critical; // NEW: 用于临界区时间判断

RT_TASK task_handle;

ec_master_t *master = NULL;
ec_domain_t *domain = NULL;
uint8_t *domain_pd = NULL;

// static unsigned int off_xctrlword;
// static unsigned int off_xstatusword;
// const static ec_pdo_entry_reg_t domain1_regs[] = {
//     {0,1, 0x00000e7c,0x00000999 , 0x6040, 0, &off_xctrlword},
//     {0,1, 0x00000e7c,0x00000999 , 0x6041, 0, &off_xstatusword},
//     {}
// };

unsigned int offset[MAX_PDO_ENTRIES];
unsigned int bit_position[MAX_PDO_ENTRIES];
static ec_pdo_entry_reg_t domain_reg[MAX_PDO_ENTRIES];


// static SEM master_sem; // NEW: RTAI 信号量
SEM task_sem_start;
SEM task_sem_done;

// 将共享内存中保存的 RXPDO/TXPDO 变量写入  domain_reg[]
void init_domain_reg(void)
{
    int i;
    int index = 0;
    for(i = 0; i < shm_ptr->rxpdo_count; i++){
        domain_reg[i].alias = shm_ptr->rxpdo[i].alias;
        domain_reg[i].position = shm_ptr->rxpdo[i].position;
        domain_reg[i].vendor_id = shm_ptr->rxpdo[i].vender_id;
        domain_reg[i].product_code = shm_ptr->rxpdo[i].product_code;
        domain_reg[i].index = shm_ptr->rxpdo[i].index;
        domain_reg[i].subindex = shm_ptr->rxpdo[i].subindex;
        domain_reg[i].offset = &offset[i];
        shm_ptr->rxpdo[i].offset = &offset[i];
        domain_reg[i].bit_position = &bit_position[i];
        shm_ptr->rxpdo[i].bit_position = &bit_position[i];
        index = i;
    }
    for (i = 0; i < shm_ptr->txpdo_count; i++) {
        domain_reg[index+i].alias = shm_ptr->txpdo[i].alias;
        domain_reg[index+i].position = shm_ptr->txpdo[i].position;
        domain_reg[index+i].vendor_id = shm_ptr->txpdo[i].vender_id;
        domain_reg[index+i].product_code = shm_ptr->txpdo[i].product_code;
        domain_reg[index+i].index = shm_ptr->txpdo[i].index;
        domain_reg[index+i].subindex = shm_ptr->txpdo[i].subindex;
        domain_reg[index+i].offset = &offset[index+i];
        shm_ptr->txpdo[i].offset = &offset[index+i];
        domain_reg[index+i].bit_position = &bit_position[index+i];
        shm_ptr->txpdo[i].bit_position = &bit_position[index+i];
    }
    domain_reg[index+i] = {0};
}

// 从 domain_pd 读取 TxPDO 数据，写到共享内存
void update_txpdo_from_domain(void)
{
    int i;
    unsigned int off;
    unsigned int bitpos;
    int value = 0;

    for (i = 0; i < shm_ptr->txpdo_count; i++) {
        off = *(shm_ptr->txpdo[i].offset);
        bitpos = *(shm_ptr->txpdo[i].bit_position);

        switch (shm_ptr->txpdo[i].bitlen) {
        case 1:
            value = EC_READ_BIT(domain_pd + off, bitpos) ? 1 : 0;
            break;
        case 8:
            value = EC_READ_U8(domain_pd + off);
            break;
        case 16:
            value = EC_READ_U16(domain_pd + off);
            break;
        case 32:
            value = EC_READ_U32(domain_pd + off);
            break;
        case 64:
            value = EC_READ_U64(domain_pd + off);
            break;
        default:
            printk(KERN_ERR "Unsupported TxPDO bitlen=%d for %s\n",
                   shm_ptr->txpdo[i].bitlen, shm_ptr->txpdo[i].name);
            continue;
        }

        shmem_write(shm_ptr->txpdo[i].name, &value);
    }
}

// 从共享内存读取 RxPDO 数据，写到 domain_pd
void update_rxpdo_to_domain(void)
{
    int i;
    unsigned int off;
    unsigned int bitpos;
    int value=0;

    for (i = 0; i < shm_ptr->rxpdo_count; i++) {
        off = *(shm_ptr->rxpdo[i].offset);
        bitpos = *(shm_ptr->rxpdo[i].bit_position);

        if (shmem_read(shm_ptr->rxpdo[i].name, &value) < 0) {
            printk(KERN_WARNING "RxPDO %s not found in shmem\n",
                   shm_ptr->rxpdo[i].name);
            continue;
        }

        switch (shm_ptr->rxpdo[i].bitlen) {
        case 1:
            EC_WRITE_BIT(domain_pd + off, bitpos, value ? 1 : 0);
            break;
        case 8:
            EC_WRITE_U8(domain_pd + off, (uint8_t)value);
            break;
        case 16:
            EC_WRITE_U16(domain_pd + off, (uint16_t)value);
            break;
        case 32:
            EC_WRITE_U32(domain_pd + off, (uint32_t)value);
            break;
        case 64:
            EC_WRITE_U64(domain_pd + off, (uint64_t)value);
            break;
        default:
            printk(KERN_ERR "Unsupported RxPDO bitlen=%d for %s\n",
                   shm_ptr->rxpdo[i].bitlen, shm_ptr->rxpdo[i].name);
            continue;
        }
    }
}



// NEW: send_callback
// void send_callback(void *cb_data)
// {
//     ec_master_t *m = (ec_master_t *) cb_data;

//     if (get_cycles() - t_last_cycle <= t_critical) {
//         rt_sem_wait(&master_sem);
//         ecrt_master_send_ext(m);
//         rt_sem_signal(&master_sem);
//     }
// }

// NEW: receive_callback
// void receive_callback(void *cb_data)
// {
//     ec_master_t *m = (ec_master_t *) cb_data;

//     if (get_cycles() - t_last_cycle <= t_critical) {
//         rt_sem_wait(&master_sem);
//         ecrt_master_receive(m);
//         rt_sem_signal(&master_sem);
//     }
// }

// EtherCAT周期任务
static void ecat_periodic_task(long arg)
{
    uint16_t data;

    while (1)
    {
        rt_sem_wait(&task_sem_start);

        // t_last_cycle = get_cycles(); // NEW: 记录周期起点

        

        // 接收过程数据
        // rt_sem_wait(&master_sem);
        ecrt_master_receive(master);
        ecrt_domain_process(domain);
        // rt_sem_signal(&master_sem);

        update_txpdo_from_domain();
        // data = shmem_read("XcontrolWord", &data);
        // EC_WRITE_U16(domain_pd + off_xctrlword, 0x0f);
        // data = EC_READ_U16(domain_pd + off_xstatusword);
        // shmem_write("XstatusWord", &data);

        update_rxpdo_to_domain();

        // 发送过程数据
        // rt_sem_wait(&master_sem);
        ecrt_domain_queue(domain);
        ecrt_master_send(master);
        // rt_sem_signal(&master_sem);

        rt_sem_signal(&task_sem_done);
    }
}

void task_func(void)
{ 
    rt_sem_signal(&task_sem_start);
    rt_sem_wait(&task_sem_done);
}

// 模块初始化
static int __init rtai_igh_init(void)
{
    // int ret, i;
    // uint32_t init_val;
    unsigned long func_ptr;
    unsigned long period = 0;

    printk(KERN_INFO "Initializing RTAI IGH EtherCAT master\n");

    // 初始化信号量
    // rt_sem_init(&master_sem, 1); // NEW
    rt_sem_init(&task_sem_start, 0);
    rt_sem_init(&task_sem_done, 0);

    // 初始化共享内存
    rtai_shmem_init();

    // 初始化EtherCAT主站
    master = ecrt_request_master(0);
    if (!master) {
        printk(KERN_ERR "Failed to request master\n");
        return -ENODEV;
    }

    // NEW: 注册回调
    // ecrt_master_callbacks(master, send_callback, receive_callback, master);

    domain = ecrt_master_create_domain(master);
    if (!domain) {
        printk(KERN_ERR "Failed to create domain\n");
        ecrt_release_master(master);
        return -ENODEV;
    }

    // printk(KERN_INFO PFX "Registering PDO entries...\n");
    init_domain_reg();
    if (ecrt_domain_reg_pdo_entry_list(domain, domain_regs)) {
        printk(KERN_ERR "PDO entry registration failed!\n");
    }

    if (ecrt_master_activate(master)) {
        printk(KERN_ERR "Failed to activate master\n");
        ecrt_release_master(master);
        return -EIO;
    }

    domain_pd = ecrt_domain_data(domain);

    // NEW: 设置 t_critical
    // t_critical = cpu_khz * 1000 / 1000 - cpu_khz * INHIBIT_TIME / 1000; 
    // 这里假设你的周期任务是 1kHz，可按需要修改

    rt_task_init_cpuid(&task_handle, ecat_periodic_task, 0, 20480, 1, 0, NULL, 1);
    rt_task_resume(&task_handle);

    period = 1000000;
    shmem_write("cycle_time", &period);

    func_ptr = (unsigned long)task_func;
    shmem_write("taskslot0", &func_ptr);

    printk(KERN_INFO "RTAI IGH initialized\n");
    return 0;
}

// 模块退出
static void __exit rtai_igh_exit(void)
{
    unsigned long zero = 0;
    shmem_write("taskslot0", &zero);
    zero = 1000000;
    shmem_write("cycle_time", &zero);

    rt_task_delete(&task_handle);

    // rt_sem_delete(&master_sem); // NEW
    rt_sem_delete(&task_sem_start);
    rt_sem_delete(&task_sem_done);

    if (master) {
        ecrt_release_master(master);
    }

    printk(KERN_INFO "RTAI IGH exited\n");
}


module_init(rtai_igh_init);
module_exit(rtai_igh_exit);

MODULE_LICENSE("GPL");
MODULE_AUTHOR("Liu_Jiankang");
MODULE_DESCRIPTION("RTAI IgH EtherCAT Master with semaphore and callbacks");
